/*
 * EKF.h
 *
 *  Created on: Apr 22, 2011
 *      Author: tflanzer
 */

#ifndef EKF_H_
#define EKF_H_

#define numStates 19
#define gpsSize 9
#define imuSize 5

// True magnetic field strength -> Stanford in 2011
// Should probably update to run world magnetic model code at initial location (Note: we do have lat/lon location from autopilot now!)
#define N_Mag_Field     0.470202056007494       // North magnetic field strength normalized to 1
#define E_Mag_Field     0.117782321161020       // East magnetic field strength normalized to 1
#define D_Mag_Field     0.874664136310760       // Down magnetic field strength normalized to 1

#include <cstdio>
#include <stdio.h>
#include <list>
#include <math.h>
#include <stdlib.h>
#include "Communication.h"
#include "execStruct.h"
#include "Matrix.h"
#include "mavlink.h"
#include "FileWriter.h"

class EKF: public Communication::AUPListener {

public:

	double dt, ts;
	FileWriter *fw;

	EKF(FileWriter *fw);
	virtual ~EKF();
	void AUPdataReceived(Communication &com);
	
	// Methods
	void initializeState();
	void getCurrentState(double *xcurr);
	void runProcessModel(double *x, double *mubt, double dt);
	void computeProcessJacobian(double *x, double *G, double dt);
	void quaternionUpdate(double *x, double wx, double wy, double wz, double dt);
	void computePredictedVariance(double *G, double *Sbart);
    void runMeasModel(double *x, double *zbt, int flag);
    void computeMeasJacobian(double *x, double *H, int flag);
    void computeKalmanGain(double *Sbart, double *H, double *Kt, int flag);        // NEED TO CHECK/UPDATE
    void computeNewMeanandCovariance(double *mubt, double *Kt, double *zbt,
    		double *Sbart, double *Ht, int flag);

    void updateState(double *mut);

    void printData(double t);


private:

	// Initial Covariance Estimate
	double P[numStates*numStates]; // State covariance estimate
	double Q_IMU_GPS[gpsSize+imuSize];	// GPS + IMU measurement noise (uncorrelated)
	double Q_IMU[imuSize];		// IMU measurement noise (uncorrelated)
	double Q_IMU_GPS_Sonar[gpsSize+imuSize+1];	// GPS + IMU + Sonar measurement noise (uncorrelated)
	double Q_IMU_Sonar[imuSize+1];		// IMU + Sonar measurement noise (uncorrelated)
	double R[numStates]; 		// Process model noise (uncorrelated)
};

#endif /* EKF_H_ */
